/**************************************************************
 * Ben Stephenson       07025656
 * Mark Renney          07010390
 * Laurie Potts         07009314
 * Michael Peeke-Vout   07014312
 *
 * CM0034 Industrial Case Project
 * Main
 * main.c
 * 28/02/2011
 * Updated - 16/05/11 
 **************************************************************/

#include <stdbool.h>
#include <ucos_ii.h>
#include <board.h>
#include <bsp.h>
#include <osutils.h>
#include <leds.h>
#include <buttons.h>
#include <lcd.h>
#include <potentiometer.h>
#include <accelerometer.h>
#include <interface.h>
#include <control.h>
#include <conveyor.h>
#include <buffer.h>
#include <robot_movements.h>
#include <messages.h>
#include <string.h>
#include <global_decs.h>

/*
*********************************************************************************************************
*                                            PRIORITIES
*********************************************************************************************************
*/

#define  APP_TASK_EMERGENCY_PRIO                   4
#define  APP_TASK_READ_CAN_PRIO                    5
#define  APP_TASK_MOVE_ARMS_PRIO                   10
#define  APP_TASK_BUTTON_READ_PRIO                 11
#define  APP_TASK_DISPLAY_PRIO                     15

/*
*********************************************************************************************************
*                                            APPLICATION TASK STACKS
*********************************************************************************************************
*/

#define  APP_TASK_MOVE_ARMS_STK_SIZE          256
#define  APP_TASK_READ_CAN_STK_SIZE           256
#define  APP_TASK_DISPLAY_STK_SIZE            256
#define  APP_TASK_CAN_STK_SIZE                256
#define  APP_TASK_BUTTON_READ_STK_SIZE        256
#define  APP_TASK_EMERGENCY_STK_SIZE          256

static OS_STK appTaskMoveArmsStk[APP_TASK_MOVE_ARMS_STK_SIZE];
static OS_STK appTaskReadCanStk[APP_TASK_READ_CAN_STK_SIZE];
static OS_STK appTaskDisplayStk[APP_TASK_DISPLAY_STK_SIZE];
static OS_STK appTaskButtonReadStk[APP_TASK_BUTTON_READ_STK_SIZE];
static OS_STK appTaskEmergencyStk[APP_TASK_CAN_STK_SIZE];

/*
*********************************************************************************************************
*                                            APPLICATION FUNCTION PROTOTYPES
*********************************************************************************************************
*/

static void appTaskMoveArms(void *pdata);
static void appTaskReadCan(void *pdata);
static void appTaskButtonRead(void *pdata);
static void appTaskDisplay(void *pdata);
static void appTaskEmergencyStop(void *pdata);
void startSystem(bool);
void initEmergency(void);

/*
*********************************************************************************************************
*                                            GLOBAL VARIABLE DEFINITIONS
*********************************************************************************************************
*/

  //Create Received Message Structure
  messageData_t received_message;
  messageData_t temp_message;
  
  int main() {

  /* Initialise the OS */
  OSInit();                                                   

  /* Create Tasks */
  startSystem(0);
  
  /* Start the OS */
  OSStart();                                                  
  
  /* Should never arrive here */ 
  return 0;      
}

/*
*********************************************************************************************************
*                                            APPLICATION TASK DEFINITIONS
*********************************************************************************************************
*
*/

/*************************************************
 * Function : appTaskMoveArms
 * Desc     : Moves the arms to collect and deposit blocks
 * Return   : Void
 * Params   : Void
 *^z**********************************************/
static void appTaskMoveArms(void *pdata) {
    
    //Define Current Pad States To Empty
    bool inPadStateFull = false;  
    bool conPadStateFull = false;
    
    while(true) {
      
      //Retrieve Current Pad States
      inPadStateFull = getPadState(IN_PAD);
      conPadStateFull = getPadState(CON_PAD);
      
      //If the robot state is idle continue with action
      if(getRobotState() == IDLE && getState() != PAUSE && getState() != STOP){
        
        //Only proceed if the inpad has item waiting
        if(inPadStateFull){
          
          //If the conveyor belt is signalling a no go keep asking until it's free
          if(conPadStateFull){
            
            //Ask conveyor if its free for a block to be deposited
            messageData_t temp_message = {ID_CONVEYOR,MY_ID,MSG_CON_SENS2_STAT_REQ};
            writeMsgToCan(&temp_message);
            OSTimeDly(200);
            continue; //reset loop
          }
          
          setRobotState(MOVING);
          //Attempt block pickup 3 times
          Default_Positions();
          for(int i = 0; i<3; ++i){
            
            //if the inpad is full, it's either the 
            //first try or last attempt failed
            if(getPadState(IN_PAD)){
              Robot_Collect_Block_Part1();
              messageData_t temp_message = {ID_CONTROLP,MY_ID,MSG_PICKUP_DEPOSIT_DONE};
              writeMsgToCan(&temp_message);
              OSTimeDly(200);
            }
            else{
              break;  //break out if successful
            }
          }
          //If still in this state 3 attempts failed, throw emergency
          if(getPadState(IN_PAD)){
            messageData_t temp_message = {ID_CONTROLP,MY_ID, MSG_EMERG_STOP};
            writeMsgToCan(&temp_message);
            continue;
          }
          
          //If here pickup was sucessful, attempt deposit 3 times
          for(int i = 0; i<3; ++i){
            
            //If empty it was first attempt or last attempt failed
            if(!getPadState(CON_PAD)){
              Robot_Collect_Block_Part2();
              messageData_t temp_message = {ID_CONVEYOR,MY_ID,MSG_PICKUP_DEPOSIT_DONE};
              writeMsgToCan(&temp_message);
              OSTimeDly(200);
            }
            else{
              break;  //Reaching here means success, break out
            }
          }
          //If this is true deposit was unsuccessful, throw emergency
          if(!getPadState(CON_PAD)){
            messageData_t temp_message = {ID_CONTROLP,MY_ID, MSG_EMERG_STOP};
            writeMsgToCan(&temp_message);
            continue;
          }
          
          setRobotState(IDLE);      
        }//inpad full
      }//inpad idle
      
      //If the system is stopping, change state to STOP
      if (getState() == STOPPING) {
        setState(STOP);
        messageData_t temp_message = {ID_CONTROLP,MY_ID, MSG_STOP};
        writeMsgToCan(&temp_message);
      }
          
      OSTimeDly(500);
    }
  
}


/*************************************************
 * Function : appTaskDisplay
 * Desc     : Writes information to the display
 * Return   : Void
 * Params   : Void
 *^z**********************************************/
static void appTaskDisplay(void *pdata){
  
  while(true){
     
     //Create message structure
     messageData_t last_message;
     getLastMsg(&last_message,MY_ID);
     
     //Set LCD screen position
     lcdSetTextPos(2,1);
     
     //Store current system state
     uint8_t sys_state = getState();
     
     //Write system status to the screen
     if (sys_state == RUNNING) {
     lcdWrite("RUNNING");
     }
     else if (sys_state == PAUSE) {
     lcdWrite("PAUSE   ");
     }
     else if (sys_state == STOP) {
     lcdWrite("STOPPED ");
     }
     else if (sys_state == STOPPING) {
     lcdWrite("STOPPING");
     }
     
     //Set LCD screen position
     lcdSetTextPos(2,2);
     lcdWrite("READ  :    WRITE");
     
     lcdSetTextPos(2,3);
     lcdWrite("T.ID : %d - %d", received_message.targetID, last_message.targetID);
     lcdSetTextPos(2,4); 
     lcdWrite("O.ID : %d - %d", received_message.originatorID, last_message.originatorID);     
     lcdSetTextPos(2,5);
     lcdWrite("READ : %d - %d", received_message.content, last_message.content);
     lcdSetTextPos(2, 6);
     lcdWrite("InPad:%0d", getPadState(IN_PAD));
     lcdSetTextPos(2, 7);
     lcdWrite("ConPad:%0d", getPadState(CON_PAD));
          
     OSTimeDly(200);
  }
}

/*************************************************
 * Function : appTaskButtonRead
 * Desc     : Reads the status of the buttons
 * Return   : Void
 * Params   : Void
 *^z**********************************************/
static void appTaskButtonRead(void *pdata) {
  
  while(true){
    
    //SIMULATE EMERGENCY
    if (isButtonPressed(JS_LEFT)) {
      initEmergency();   
    }
    //SIMULATE PAUSE
    if (isButtonPressed(JS_UP)) {
      setState(PAUSE);   
    }
    //SIMULATE STOP
    else if (isButtonPressed(JS_RIGHT)) {
      setState(STOPPING);   
    }
    //SIMULATE RESUME
    else if (isButtonPressed(JS_DOWN)) {
      setState(RUNNING);   
    }
    
    //TOGGLE INPUT PAD SENSOR
    else if (isButtonPressed(BUT_1)) {
      if(getPadState(IN_PAD)){
        setPadState(IN_PAD,false);
        OSTimeDly(150);
      }
      else {
        setPadState(IN_PAD,true);
        OSTimeDly(150);
      }
    }
    
    //TOGGLE CONVEYOR SENSOR
    else if (isButtonPressed(BUT_2)) {
      if(getPadState(CON_PAD)){
        setPadState(CON_PAD,false);
        OSTimeDly(20);
      }
      else {
        setPadState(CON_PAD,true);
        OSTimeDly(20);
      }
    }
       
   OSTimeDly(20);
  }
}


/*************************************************
 * Function : appTaskReadCan
 * Desc     : Reads and issues actions based on CAN messages
 * Return   : Void
 * Params   : Void
 *^z**********************************************/
static void appTaskReadCan(void *pdata) {
     
  //Send ready message to control panel
  messageData_t temp_message = {ID_CONTROLP,MY_ID,MSG_READY};
  writeMsgToCan(&temp_message);
  
  while(true){
  
  if(readMsgFromCan(&received_message)) {
        
        //Create message structure
        messageData_t lastWrite;
        getLastMsg(&lastWrite, MY_ID);
        
        //Control panel showing inpad is full
        if((received_message.content == MSG_SENS1_FULL)) {
          if (received_message.originatorID == ID_CONTROLP) {
            
            setPadState(IN_PAD, true);
            sendACK(ID_CONTROLP);
            
          }
        }
        
        //Control panel showing inpad is empty
        if((received_message.content == MSG_SENS1_EMPTY)) {
          if (received_message.originatorID == ID_CONTROLP) {
            setPadState(IN_PAD, false);
          }
        }
        
        //Message received saying conveyor is free
        else if((received_message.content == MSG_SENS2_EMPTY) &&
          (received_message.originatorID == ID_CONVEYOR)) {
            setPadState(CON_PAD, false);
        }
        
        //Message received saying conveyor is full
        if((received_message.content == MSG_SENS2_FULL)) {
          if (received_message.originatorID == ID_CONVEYOR) {
            setPadState(CON_PAD, true);
          }
        }
        
        //If a emergency stop message occurs
        else if(received_message.content == MSG_EMERG_STOP) {
            initEmergency();
        }
        
        //If a pause message occurs
        else if(received_message.content == MSG_PAUSE) {
            setState(PAUSE);
            OSTimeDly(MY_ID*10);
            messageData_t temp_message = {ID_CONTROLP,MY_ID, MSG_PAUSE};
            writeMsgToCan(&temp_message);
        }
        
        //If resume message occurs
        else if(received_message.content == MSG_RESUME) {
            setState(RUNNING);
            OSTimeDly(MY_ID*10);
            messageData_t temp_message = {ID_CONTROLP,MY_ID, MSG_RESUME};
            writeMsgToCan(&temp_message);
        }
        
        //If stop message occurs
        else if(received_message.content == MSG_STOP) {
            setState(STOPPING);
        }
      } 
   
   OSTimeDly(10);
  }
}

/*************************************************
 * Function : initEmergency
 * Desc     : Initiate task to handle emergency stop.
 * Return   : Void
 * Params   : Void
 *^z**********************************************/
void initEmergency(void){
  
  //conveyorSetState(CONVEYOR_OFF);
  OSTaskCreate(appTaskEmergencyStop,                               
               (void *)0,
               (OS_STK *)&appTaskEmergencyStk[APP_TASK_EMERGENCY_STK_SIZE - 1],
               APP_TASK_EMERGENCY_PRIO);
}

/*************************************************
 * Function : appTaskEmergencyStop
 * Desc     : Kill system and notify user vua GUI.
 * Return   : Void
 * Params   : Void
 *
 * TODO     : Reset on button press?
 *^z**********************************************/
void appTaskEmergencyStop(void *pdata){
   
  //Kill this to ensure can messages have no further control of system state
  OSTaskDel(APP_TASK_READ_CAN_PRIO);
  OSTaskDel(APP_TASK_MOVE_ARMS_PRIO);
  OSTaskDel(APP_TASK_DISPLAY_PRIO);
  OSTaskDel(APP_TASK_BUTTON_READ_PRIO);
  
  //Delay message response to be related to ID to avoid flooding message buffer
  OSTimeDly(MY_ID*10);
  
  //Tell control panel this component is in emergency state
  messageData_t tempWrite = {ID_CONTROLP, MY_ID, MSG_EMERG_STOP};
  writeMsgToCan(&tempWrite); 
  
  //Create last message structures
  messageData_t lastWrite;
  getLastMsg(&lastWrite, MY_ID);
  messageData_t last_message;
  getLastMsg(&last_message,MY_ID);
  
  //notify user of emergency situation
  printf( "\f" );
     
  //Write latest information to the screen
  lcdSetTextPos(2,2);
  lcdWrite("READ  :    WRITE");
  lcdSetTextPos(2,3);
  lcdWrite("T.ID : %d - %d", received_message.targetID, last_message.targetID);
  lcdSetTextPos(2,4); 
  lcdWrite("O.ID : %d - %d", received_message.originatorID, last_message.originatorID);     
  lcdSetTextPos(2,5);
  lcdWrite("READ : %d - %d", received_message.content, last_message.content);
  
  //Write shutdown message on LCD
  lcdSetTextPos(2, 7);
  lcdWrite("SHUTDOWN");
  lcdSetTextPos(2, 10);
  lcdWrite("HOLD BUT2 FOR RESET");
  
  //Change LED state
  ledSetState(USB_LINK_LED, LED_OFF);
  ledSetState(USB_CONNECT_LED, LED_ON);
  
  //flag for screen flash
  bool flash = false;
  
  while(true){
    
    //If a reset occurs
    if (isButtonPressed(BUT_2)) {
      printf( "\f" );
      ledSetState(USB_LINK_LED, LED_OFF);
      ledSetState(USB_CONNECT_LED, LED_OFF);
      //Restart the system
      startSystem(1);
      OSTaskDel(OS_PRIO_SELF);
      break;
    }
    
    //flash screen
    lcdSetTextPos(2, 8);
    if(flash){
      lcdWrite("                    ");
      flash = false;
    }
    else{
      
      lcdWrite("EMERGENCY STOP!");
      flash = true;
    }
    
    //flash LEDs
    ledToggle(USB_LINK_LED);
    ledToggle(USB_CONNECT_LED);
           
    OSTimeDly(1000);
  }
}

/*************************************************
 * Function : startSystem
 * Desc     : Initialise system and create system tasks
 * Return   : Void
 * Params   : bool
 *^z**********************************************/
void startSystem(bool started){
  
  //First time initialise board/robot/tick
  if(!started){
    bspInit();
    robotInit();
    osStartTick();
  }
  
  //Initialise arm flags to empty system
  setState(RUNNING);
  initRobotVars();
  setPadState(IN_PAD,false);
  setPadState(CON_PAD,false);
  
  //Create move arms task
  OSTaskCreate(appTaskMoveArms,                               
               (void *)0,
               (OS_STK *)&appTaskMoveArmsStk[APP_TASK_MOVE_ARMS_STK_SIZE - 1],
               APP_TASK_MOVE_ARMS_PRIO);
  
  //Create read CAN task
  OSTaskCreate(appTaskReadCan,                               
               (void *)0,
               (OS_STK *)&appTaskReadCanStk[APP_TASK_READ_CAN_STK_SIZE - 1],
               APP_TASK_READ_CAN_PRIO);
  
  //Create button read task
  OSTaskCreate(appTaskButtonRead,                               
               (void *)0,
               (OS_STK *)&appTaskButtonReadStk[APP_TASK_BUTTON_READ_STK_SIZE - 1],
               APP_TASK_BUTTON_READ_PRIO);

  //Create display task
  OSTaskCreate(appTaskDisplay,                               
               (void *)0,
               (OS_STK *)&appTaskDisplayStk[APP_TASK_DISPLAY_STK_SIZE - 1],
               APP_TASK_DISPLAY_PRIO);
  
}